#include <string>
#include <iostream>

#include <opencv2/core.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp>


#include"evaluator.h"
#include"tracker_v1_eccv_final.h"

void test() {
	cv::Mat img = cv::imread("d:/data/a_regular0000.png");
	cv::Point2f center(img.rows / 2, img.cols / 2);

	cv::Matx23f A = cv::getRotationMatrix2D(center, 15.0, 1.0);
	cv::Mat trans;
	cv::warpAffine(img, trans, A, img.size(), cv::INTER_LINEAR, cv::BORDER_CONSTANT, cv::Scalar(0));

	cv::imshow("frame", img);
	cv::imshow("trans", trans);
	cv::waitKey();
}

static Matx33f getLogic_K()
{
	cv::Matx33f K = cv::Matx33f::zeros();
	K(0, 0) = 832.560791f;
	K(0, 2) = 319.5f;
	K(1, 1) = 832.560791f;
	K(1, 2) = 239.5f;
	K(2, 2) = 1.0f;
	return K;
}

int test_live(const std::string& rbot_root)
{
	cv::Matx33f K = getLogic_K();

	std::string dataPath = rbot_root;
	std::vector<Pose> gtPoses = read_gt_poses(dataPath + "poses_first.txt");

#if 0
	std::vector<std::string> body_names{
		"ape",  "bakingsoda", "benchviseblue", "broccolisoup", "cam",
		"can",  "cat",        "clown",         "cube",         "driller",
		"duck", "eggbox",     "glue",          "iron",         "koalacandy",
		"lamp", "phone",      "squirrel" };

	CVRModel model(dataPath + modelName + "/" + modelName + ".obj");
#else
	std::string modelName = "cat";
	CVRModel model(dataPath + modelName + "/" + modelName + ".obj");
#endif

	VisHandler visHdl(model, K);

	std::string imgDir = dataPath + modelName + "/frames/";

	auto loadImage = [&imgDir](int fi) {
		auto file = imgDir + ff::StrFormat("a_regular%04d.png", fi);
		//auto file = imgDir + ff::StrFormat("b_dynamiclight%04d.png", fi);
		//auto file = imgDir + ff::StrFormat("c_noisy%04d.png", fi);
		return cv::imread(file, cv::IMREAD_COLOR);
	};

	Tracker tracker;
	tracker.loadModel(model.getFile());

	int start = 0;
	Pose pose = gtPoses[start];
	tracker.reset(loadImage(start), pose, K);

	int nLost = 0, nTotal = 0, totalTime = 0;

	int camera_id = 0;
	cv::VideoCapture cap(camera_id, cv::CAP_DSHOW);
	if (!cap.isOpened()) {
		std::cout << "[ERROR] fail to open video file. cam_id = " << camera_id << std::endl;
		return -1;
	}

	cv::Mat frame;
	bool start_track = false;
	for (int fi = 0;; ++fi) {
		cap >> frame;

		if (start_track) {
			time_t beg = clock();
			tracker.startUpdate(frame, gtPoses[fi]);

			auto R0 = pose.R;
			tracker.update(pose);

			tracker.endUpdate(pose);
			totalTime += int(clock() - beg);
			printf("fi=%d, time=%dms     \r", fi, int(clock() - beg));
		}
		
		visHdl.draw(frame, pose.R, pose.t);

		int key = cv::waitKey(1);
		if (27 == key)
			break;
		if ('t' == key || 'T' == key) {
			start_track = true;
		}
	}

	return 0;
}


int main(int argc, char* argv[]) {
	return test_live(argv[1]);
}